#include <gtk/gtk.h>
#include <cmath>
#include "structure.h"
#include "support.h"
#include "draw.h"
#include "const.h"

inline GdkColor set_color(gushort r, gushort g, gushort b,GdkGC *gc)
{
  GdkColor color;

  color.red = r, color.green = g, color.blue = b;
  gdk_color_alloc(gdk_colormap_get_system(),&color);
  gdk_gc_set_foreground(gc, &color);
  return color;
}
inline void set_color2(GdkColor color, GdkGC *gc)
{
  gdk_color_alloc(gdk_colormap_get_system(),&color);
  gdk_gc_set_foreground(gc, &color);
}
GdkColor make_color(gushort r, gushort g, gushort b)
{
  GdkColor color;
  color.red = r; color.green = g; color.blue = b;
  return color;
}

// Point構造体用
void translate_map_to_screen(GuiInterface *gui, Point *dst, Point *src, int width, int height)
{
  //fprintf(stderr, "resolution: %lf\n", gui->screen_resolution);
  //screen_offset_x = (screen_offset_x<0) ? width +screen_offset_x : screen_offset_x;
  //screen_offset_y = (screen_offset_y<0) ? height+screen_offset_y : screen_offset_y;
  dst->x = gui->screen_offset_x + width*0.5  + src->x/gui->screen_resolution;
  dst->y = gui->screen_offset_y + height*0.5 - src->y/gui->screen_resolution;
  dst->theta = src->theta;
}
// LocalizerParticle構造体用
void translate_map_to_screen(GuiInterface *gui, LocalizerParticle *dst, LocalizerParticle *src, int width, int height)
{
  //fprintf(stderr, "resolution: %lf\n", gui->screen_resolution);
  //screen_offset_x = (screen_offset_x<0) ? width +screen_offset_x : screen_offset_x;
  //screen_offset_y = (screen_offset_y<0) ? height+screen_offset_y : screen_offset_y;
  dst->x = gui->screen_offset_x + width*0.5  + src->x/gui->screen_resolution;
  dst->y = gui->screen_offset_y + height*0.5 - src->y/gui->screen_resolution;
  dst->theta = src->theta;
  dst->weight = src->weight;
}


void draw_area_initialize(GuiInterface *gui)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;

  //gdk_threads_enter();
  gdk_draw_rectangle(pixmap1, drawingarea1->style->white_gc,
                     TRUE, 0, 0, width, height);
  gdk_gc_set_line_attributes(gui->gc, 2, GDK_LINE_SOLID, GDK_CAP_ROUND,
                             GDK_JOIN_ROUND);
  //gdk_threads_leave();
}

void draw_point(GuiInterface *gui, Point *pose, int point_size, GdkColor color)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  Point p1;

  //gdk_threads_enter();
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  translate_map_to_screen(gui, &p1, pose, width, height);
  set_color2(color, gui->gc);
  gdk_draw_arc(pixmap1, gui->gc, 1, p1.x , p1.y,
	       point_size, point_size, 0, 360*64);
  //gdk_threads_leave();
}

/**
 * @param gui GUIデータ
 * @param laser LRFデータ
 * @param pose LRFの位置，姿勢[m,rad]
 */
void draw_laser_range(GuiInterface *gui,
		      LaserRangeData *laser, LocalizerParticle *pose)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  int i;
  double angle;
  LocalizerParticle p1,p2;
  
  //gdk_threads_enter();
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  for(i=0; i<laser->total_points; i++) {
    //angle = (double)i*laser->resolution + laser->offset_fov + pose->theta/180.0*M_PI;
    angle = (double)i*laser->resolution + laser->offset_fov + pose->theta;
    p1.x = pose->x + laser->range[i] * cos(angle);
    p1.y = pose->y + laser->range[i] * sin(angle);
    translate_map_to_screen(gui, &p2, &p1, width, height);
    set_color(0xffff, 0x0000, 0x0000, gui->gc);
    gdk_draw_arc(pixmap1, gui->gc, 1, p2.x , p2.y,
 		 6, 6, 0, 360*64);
  }
  //gdk_threads_leave();
}


void show_draw_area(GuiInterface *gui)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  gtk_widget_queue_draw_area(drawingarea1,
			     0, 0,
			     width, height);
}

void get_draw_size(GuiInterface *gui, int *width, int *height)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  *width = drawingarea1->allocation.width;
  *height = drawingarea1->allocation.height;
}


// パーティクル
void draw_particle(GuiInterface *gui,
		   LocalizerParticle *particle,
		   int point_size,
		   GdkColor particle_color,
		   GdkColor vector_color)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  LocalizerParticle p,p2;

  translate_map_to_screen(gui, &p, particle, width, height);
  // p.theta * -1 mean acording to flip x and y
  //p2.x = 15*cos( - (p.theta+90)/180*M_PI ) + p.x;
  //p2.y = 15*sin( - (p.theta+90)/180*M_PI ) + p.y;
  p2.x = 15*cos( - (p.theta+DEG2RAD(90)) ) + p.x;
  p2.y = 15*sin( - (p.theta+DEG2RAD(90)) ) + p.y;
  //  gray =  0xffff - 10*(0xffff * p.weight);
  //	  set_color(gray,gray,gray, gui->gc);

  //gdk_threads_enter();
  set_color2( particle_color, gui->gc);
  gdk_draw_arc(pixmap1,gui->gc, 1, p.x - 5,  p.y - 5,
	       point_size, point_size, 0, 360*64);
  
  set_color2(vector_color, gui->gc);  
  gdk_draw_line(pixmap1, gui->gc, p.x, p.y, p2.x, p2.y);
//  gdk_draw_arc(pixmap1,gui->gc,1 , p.x,p.y,
  //	       2,2,0,360*64);
  
  //gdk_threads_leave();
}
void draw_particles(GuiInterface *gui,
		    LocalizerParticle *particles,
		    int n, int point_size)
{
  int i;
  for(i=0; i < n ; i++){
    draw_particle( gui, &particles[i], point_size,
		   make_color(0xae00,0xad00,0xcd00),
		   make_color(0x0, 0x0 ,0x0));
  }
}

// マップ
void draw_pointmap(PointMap *pmap, GuiInterface *gui)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  int i;
  Point p;

  //gdk_threads_enter();
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  for( i=0; i < pmap->numdata ; i++){
    set_color(0x0, 0x0, 0xffff,gui->gc);
    translate_map_to_screen(gui, &p, &pmap->data[i], width, height);
    gdk_draw_arc(pixmap1, gui->gc, 1, p.x, p.y, 4, 4, 0, 360*64);
  }
  //gdk_threads_leave();
}
